#include <iostream>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/spi/spidev.h>
#include <cstring>

using namespace std;

typedef enum
{
    MOTOR1 = 1,
    MOTOR2,
	MOTOR3,
    MOTOR4,
    MOTOR5,
}motor_e;

typedef enum
{
    FDCAN2 = 1,
    FDCAN3 = 2,
}fdcan_dev;

void spi_2_fdcan_start(uint32_t freq);
void spi_2_fdcan_stop(void);
void spi_2_fdcan_send(uint8_t dev, uint16_t id, uint8_t *pdata, uint8_t length, uint32_t speed = 4000000);
uint8_t spi_2_fdcan_receive(uint8_t dev, uint16_t *id, uint8_t* pdata, uint32_t freq = 4000000);
void set_position_32(motor_e motor, int32_t pos, int32_t vel, int32_t torque);

int spi_fd;

int main()
{
    int length;
    uint16_t id;
    uint8_t recv[64];
    uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
    spi_2_fdcan_start(4000000);
    spi_2_fdcan_send(FDCAN2, 0x8001, cmd, sizeof(cmd), 4000000);
    length = spi_2_fdcan_receive(FDCAN2, &id, recv, 4000000);
    if(length > 0)
    {
        printf("RECV:");
        for(int i = 0; i < length; i++)
        {
            printf("0x%02x,", recv[i]);
        }
        printf("\b\n");
    }
    spi_2_fdcan_stop();
    return 0;
}

void spi_2_fdcan_start(uint32_t freq)
{
    int ret;
    //使用SPI1.0
    spi_fd = open("/dev/spidev0.0", O_RDWR);
    if (spi_fd < 0) {
        perror("Error: Cann't open SPI Dev.\n");
        return;
    }

    //配置SPI参数
    uint32_t speed = freq;
    uint16_t delay = 0;
    uint8_t bits_per_word = 8;
    uint8_t mode = 0;
    ret = ioctl(spi_fd, SPI_IOC_WR_MODE, &mode);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_MODE fault.\n");
        return;
    }

    ret = ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &bits_per_word);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_BITS fault.\n");
        return;
    }

    ret = ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
    if (ret == -1) {
        perror("Error: SPI_IOC_WR_MAX_SPEED fault.\n");
        return;
    }
}

void spi_2_fdcan_stop(void)
{
    close(spi_fd);
}

void spi_2_fdcan_send(uint8_t dev, uint16_t id, uint8_t *pdata, uint8_t length, uint32_t speed)
{
    int ret;
    //发送数据
    uint8_t tx_buf[64];
    tx_buf[0] = 0x80 | dev;
    struct spi_ioc_transfer spi { };
    spi.tx_buf = (unsigned long)tx_buf;
    spi.len = 1;
    spi.delay_usecs = 0;
    spi.speed_hz = speed;
    spi.bits_per_word = 8;
    // Send WR_ADDR
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) {
        perror("Error: SPI_IOC_MESSAGE fault.\n");
        return;
    }

    spi.tx_buf = (unsigned long)&id;
    spi.len = 2;
    // Send CANID
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) 
    {
        perror("Error");
    }

    spi.tx_buf = (unsigned long)&length;
    spi.len = 1;
    // Send length
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1)
    {
        perror("Error");
    }

    memcpy(tx_buf, pdata, length);

    spi.tx_buf = (unsigned long)tx_buf;
    spi.len = length;
    // Send message
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1)
    {
        perror("Error");
    }
}

uint8_t spi_2_fdcan_receive(uint8_t dev, uint16_t *id, uint8_t* pdata, uint32_t freq)
{
    int ret;
    uint8_t length;
    //发送数据
    uint8_t tx_buf[64];
    uint8_t rx_buf[64];

    tx_buf[0] = dev;
    struct spi_ioc_transfer spi { };
    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = 1;
    spi.delay_usecs = 0;
    spi.speed_hz = freq;
    spi.bits_per_word = 8;
    // Send wr_addr
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) {
        perror("Error: SPI_IOC_MESSAGE fault.\n");
        return -1;
    }

    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = 2;
    // Read CAN-ID
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1) 
    {
        perror("Error");
    }

    *id = *(uint16_t*)rx_buf;

    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = 1;
    // Read length
    ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
    if (ret < 1)
    {
        perror("Error");
    }

    length = rx_buf[0];

    spi.tx_buf = (unsigned long)tx_buf;
    spi.rx_buf = (unsigned long)rx_buf;
    spi.len = length;

    if(spi.len != 0)
    {
        // Read Message
        ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi);
        if (ret < 1)
        {  
            perror("Error");
        }
        else
        {
            memcpy(pdata, rx_buf, length);
        }
    }
    return length;
}

// void set_position_32(motor_e motor, int32_t pos, int32_t vel, int32_t torque)
// {
//     __IO uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
//     *(int32_t*)&cmd[5] = pos;
//     *(int32_t*)&cmd[9] = vel;
//     *(int32_t*)&cmd[13] = torque;
//     push_queue(motor, (uint8_t*)cmd, sizeof(cmd));
// }

// void set_velocity_32(motor_e motor, int32_t vel, int32_t torque)
// {
//     uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
//     *(uint32_t*)&cmd[5] = 0x80000000;
//     *(int32_t*)&cmd[9] = vel;
//     *(int32_t*)&cmd[13] = torque;
//     push_queue(motor, cmd, sizeof(cmd));
// }
